package com.dmatek.uwb.util.tool;

import java.util.ArrayList;
import java.util.List;

import com.dmatek.uwb.local.algorithm.LocalAlgorithm;
import com.dmatek.uwb.packet.bean.CrossPoint;
import com.dmatek.uwb.packet.bean.ENodeBDistance;
import com.dmatek.uwb.packet.bean.IterateDataFormat;
import com.dmatek.uwb.packet.bean.CrossPoint.SectionSituation;
import com.dmatek.uwb.packet.bean.Vector;
import com.dmatek.uwb.xml.dom.bean.BaseNode;
import com.dmatek.uwb.xml.dom.bean.BasePath;
import com.dmatek.uwb.xml.dom.bean.BranchNode;
import com.dmatek.uwb.xml.dom.bean.BranchPath;
import com.dmatek.uwb.xml.dom.bean.Divider;
import com.dmatek.uwb.xml.dom.bean.Point;
import com.dmatek.uwb.xml.dom.bean.Region;
import com.dmatek.uwb.xml.dom.bean.Side;
/***
 * 几何工具类
 * @author Administrator
 * @data 2019年6月22日 下午3:04:52
 * @Description
 */
@SuppressWarnings("deprecation")
public final class GeometryToolkit {
	public static double max(double... params) {
		double rmax = Double.MIN_VALUE;
		for (double param : params) {
			if(param >= rmax) {
				rmax = param;
			}
		}
		return rmax;
	}
	/***
	 * 获取三角形最大角
	 * @param pa
	 * @param pb
	 * @param pc
	 * @return
	 */
	public static double obtainMaxAngle(Point pa, Point pb, Point pc) {
        // 计算三角形各个边长
        double ra = pixelPitch(pb, pc);
        double rb = pixelPitch(pa, pc);
        double rc = pixelPitch(pa, pb);
        // 比较最大的边
        double rmax = max(ra, rb, rc);
        double cos = 0;
        if (rmax == ra) { // cosA = (b * b + c * c - a * a)/2 * b * c;
            cos = (rb * rb + rc * rc - ra * ra) / (2 * rb * rc);
        } else if (rmax == rb) {  // cosB = (a * a + c * c - b * b)/2 * a * c;
            cos = (ra * ra + rc * rc - rb * rb) / (2 * ra * rc);
        } else if (rmax == rc) {// // cosC = (a * a + b * b - c * c)/2 * a * b;
            cos = (ra * ra + rb * rb - rc * rc) / (2 * ra * rb);
        }
        return Math.acos(cos) * 180 / Math.PI;
    }
	/***
	 * 计算两点之间的距离
	 * @param start
	 * @param end
	 * @return
	 */
	public static double pixelPitch(Point start, Point end) {
        return Math.sqrt((start.getX() - end.getX()) * (start.getX() - end.getX()) + 
        		         (start.getY() - end.getY()) * (start.getY() - end.getY()));
    }
	/***
	 * 两条线的交点坐标
	 * @param sp1
	 * @param ep1
	 * @param sp2
	 * @param ep2
	 * @return
	 */
    public static Point twoLineIntersectionPoint(Point sp1, Point ep1, Point sp2, Point ep2) {
        /*
         * 1. 直线1的一般式：A*x + B*y + C = 0  => 
         *       A=Y2-Y1
         *       B=X1-X2
         *       C=X2*Y1-X1*Y2
         *  交点：((b1*c2-b2*c1)/(a1*b2-a2*b1),(a2*c1-a1*c2)/(a1*b2-a2*b1))
         */
        double a1, a2, b1, b2, c1, c2;
        a1 = ep1.getY() - sp1.getY();
        a2 = ep2.getY() - sp2.getY();
        b1 = sp1.getX() - ep1.getX();
        b2 = sp2.getX() - ep2.getX();
        c1 = (ep1.getX()) * (sp1.getY()) - (sp1.getX()) * (ep1.getY());
        c2 = (ep2.getX()) * (sp2.getY()) - (sp2.getX()) * (ep2.getY());
        double px = (double)(b1 * c2 - b2 * c1) / (a1 * b2 - a2 * b1);
        double py = (double)(a2 * c1 - a1 * c2) / (a1 * b2 - a2 * b1);
        if (a1 * b2 - a2 * b1 == 0) {// 没有交点
            return new Point(-1, -1);
        } else {
            return new Point(px, py);
        }
    }
    /***
     * 判断点是否在线上
     * @param p
     * @param line
     * @return
     */
    public static boolean onSegment(Point p, BasePath line) {
        if (((p.getX() >= line.getStart().getX() && p.getX() <= line.getEnd().getX()) ||
             (p.getX() >= line.getEnd().getX() && p.getX() <= line.getStart().getX())) 
          && ((p.getY() >= line.getStart().getY() && p.getY() <= line.getEnd().getY()) ||
             (p.getY() >= line.getEnd().getY() && p.getY() <= line.getStart().getY()))) {
            return true;
        }
        return false;
    }
	/***
	 * 判断直线是否进过区域
	 * @param region
	 * @param line
	 * @return
	 */
    public static boolean isLineAcrossRegion(Region region, BasePath line) {
        // 只要直线和区域有两个交点，就认为直线穿过区域
        Point o1, o2, o3, o4;
        Side top, right, bottom, left;
        top = new Side(region.getStart(), new Point(region.getEnd().getX(), region.getStart().getY()));
        right = new Side(new Point(region.getEnd().getX(), region.getStart().getY()), region.getEnd());
        bottom = new Side(new Point(region.getStart().getX(), region.getEnd().getY()), region.getEnd());
        left = new Side(region.getStart(), new Point(region.getStart().getX(), region.getEnd().getY()));
        o1 = twoLineIntersectionPoint(line.getStart(), line.getEnd(), top.getStart(), top.getEnd());
        o2 = twoLineIntersectionPoint(line.getStart(), line.getEnd(), right.getStart(), right.getEnd());
        o3 = twoLineIntersectionPoint(line.getStart(), line.getEnd(), bottom.getStart(), bottom.getEnd());
        o4 = twoLineIntersectionPoint(line.getStart(), line.getEnd(), left.getStart(), left.getEnd());
        boolean[] segments = new boolean[4];
        segments[0] = onSegment(o1, top);
        segments[1] = onSegment(o2, right);
        segments[2] = onSegment(o3, bottom);
        segments[3] = onSegment(o4, left);
        int tk = 0;
        for(boolean flag:segments) {
            if (flag) {
                tk ++;
            }
        }
        return tk >= 2 ? true : false;
    }
	/***
	 * 检测两个坐标点是否在分割线的一侧
	 * @param t1
	 * @param t2
	 * @param divider
	 * @return
	 */
	public static boolean checkSide(Point t1, Point t2, Divider divider) {
        double m = 0;
        if(divider.getStart().getX() == divider.getEnd().getX()) { 
        	// 直线方程为x - divider.Start.X = 0;
            m = (t1.getX() - divider.getStart().getX()) * (t2.getX() - divider.getStart().getX());
        } else if (divider.getStart().getY() == divider.getEnd().getY()) { 
        	// 直线方程为y - divider.Start.Y = 0;
            m = (t1.getY() - divider.getStart().getY()) * (t2.getY() - divider.getStart().getY());
        } else { 
        	// 直线方程为(divider.End.Y - divider.Start.Y)*x + (divider.Start.X - divider.End.X)*y + (divider.End.X) * (divider.Start.Y) - (divider.Start.X) * (divider.End.Y) = 0
            m = ((divider.getEnd().getY() - divider.getStart().getY()) * t1.getX() + 
            	(divider.getStart().getX() - divider.getEnd().getX()) * t1.getY() + 
                (divider.getEnd().getX()) * (divider.getStart().getY()) - 
                (divider.getStart().getX()) * (divider.getEnd().getY())) * 
                ((divider.getEnd().getY() - divider.getStart().getY()) * t2.getX() + 
                (divider.getStart().getX() - divider.getEnd().getX()) * t2.getY() + 
                (divider.getEnd().getX()) * (divider.getStart().getY()) - 
                (divider.getStart().getX()) * (divider.getEnd().getY()));
        }
        return m > 0 ? true:false;
    }
	/***
	 * 判断坐标和哪一个基站属于一边
	 * @param opts
	 * @param suitable
	 * @param other
	 * @param commonDivider
	 * @return
	 */
	public static int checkSide(Point[] opts, BaseNode suitable, 
			BaseNode other, Divider commonDivider) {
        // 需要判断两个焦点在同侧还是处在异侧
        if (opts.length == 1) { // 只有一个坐标时不需要进行权值的叠加
            if (opts[0].getX() <= 0 && opts[0].getY() <= 0) {
                return -1;
            }
            if(checkSide(opts[0], new Point(suitable.getX(), suitable.getY()), commonDivider)) {
                return 1;
            } else if (checkSide(opts[0], new Point(other.getX(), other.getY()), commonDivider)) {
                return 0;
            }
        } else if (opts.length == 2) { // 有两个坐标
            // 需要判断两个坐标是否在同一侧
            boolean mark1 = checkSide(opts[0], new Point(suitable.getX(), suitable.getY()), commonDivider);
            boolean mark2 = checkSide(opts[1], new Point(suitable.getX(), suitable.getY()), commonDivider);
            if (mark1 && mark2) { // 都在基站1的一侧
                return 1;
            } else if (!mark1 && !mark2) {// 都在基站2的一侧
                return 0;
            } else { // 基站1和基站2都存在1个，根据基站权重判断
                String priorityId = commonDivider.obtainPriority(suitable.getId(), other.getId());
                if (!StringUtil.isEmpty(priorityId)) {
                    if (priorityId.equals(suitable.getId())) {
                        return 1;
                    } else {
                        return 0;
                    }
                }
            }
        }
        return -1;
    }
	/***
	 * 计算圆最优交点坐标
	 * @return
	 */
	public static CrossPoint calculateCircleOptimalPoints(Point c1, Point c2, float r1, float r2) {
		CrossPoint crossPoint = calculateCircleCrossPoints(c1, c2, r1, r2);
		if(crossPoint.getSectionSituation() == SectionSituation.NO) { 
			// 说明没有任何交点时，说明两个圆的关系，应该求它们的最优解才行
			List<IterateDataFormat> iteratedatas = new ArrayList<IterateDataFormat>();
			iteratedatas.add(new IterateDataFormat(c1.getX(), c1.getY(), r1));
			iteratedatas.add(new IterateDataFormat(c2.getX(), c2.getY(), r2));
			IterateDataFormat optIterator = LocalAlgorithm.INSTANCE.dimenIndoorPosAlgorithm(iteratedatas);
			return new CrossPoint(new Point(optIterator.getX(),  optIterator.getY()));
		}
		return crossPoint;
	}
	/***
	 *  是否在房间里面
	 * @param pts
	 * @param roomNode
	 * @param aisleNode
	 * @param divider
	 * @return
	 */
	public static int isRoomWithAisle(Point[] pts, BaseNode roomNode, BaseNode aisleNode, Divider divider)
    {
        if (null != pts && pts.length > 0) {
            if (pts.length == 1) {
                boolean mk1 = checkSide(new Point(roomNode.getX(), roomNode.getY()), pts[0], divider);
                boolean mk2 = checkSide(new Point(aisleNode.getX(), aisleNode.getY()), pts[0], divider);
                return mk1 ? 1 : (mk2 ? 0 : -1);
            } else if (pts.length == 2) {
            	boolean mk1 = checkSide(new Point(roomNode.getX(), roomNode.getY()), pts[0], divider);
            	boolean mk2 = checkSide(new Point(roomNode.getX(), roomNode.getY()), pts[1], divider);
            	boolean nk1 = checkSide(new Point(aisleNode.getX(), aisleNode.getY()), pts[0], divider);
            	boolean nk2 = checkSide(new Point(aisleNode.getX(), aisleNode.getY()), pts[1], divider);
                if (mk1 && mk2) {
                    return 1;
                } else if (nk1 && nk2) {
                    return 0;
                } else { // 说明1边存在一个需要我们根据优先权判断
                    String priorityId = divider.obtainPriority(roomNode.getId(), aisleNode.getId());
                    if (!StringUtil.isEmpty(priorityId)) {
                        if (priorityId.equals(roomNode.getId())) {
                            return 1;
                        } else {
                            return 0;
                        }
                    }
                }
            }
        }
        return -1;
    }
	/***
	 * 计算圆交点坐标
	 * @param c1
	 * @param c2
	 * @param r1
	 * @param r2
	 * @return
	 */
	public static CrossPoint calculateCircleCrossPoints(Point c1, Point c2, float r1, float r2) {
		double k1, k2, k3, k4, k5, k6, k7, k8, p1;
		CrossPoint crossPoint = new CrossPoint();
		/*
		 * 两条直线的交线方程为:2(X1 - X2)X + 2（Y1 - Y2）Y = R2^2 - R1^2 + X1^2 - X2^2 +
		 * Y1^2 - Y2^2;
		 */
		k1 = Math.pow(r2, 2) - Math.pow(r1, 2) + Math.pow(c1.getX(), 2)
				- Math.pow(c2.getX(), 2) + Math.pow(c1.getY(), 2)
				- Math.pow(c2.getY(), 2);
		if (c1.getY() == c2.getY()) { 
			c1.setY(c1.getY() + 1);
		}
        if (c1.getX() == c2.getX()) {
        	c1.setX(c1.getX() + 1);
        }
		k2 = (double) k1 / (double) (2 * (c1.getY() - c2.getY()));
		k3 = (double) (c1.getX() - c2.getX()) / (double) (c1.getY() - c2.getY());
		/*
		 * 此时的直线方程为 Y = K2 - K3 * X; 将直线方程带入圆的方程中 (X-Xa) ^ 2 + (Y-Ya) ^ 2 = Ra ^ 2;
		 * 得到的一元二次方程为:(X-Xa) ^ 2 + (K2 - K3X-Ya) ^ 2 = Ra ^ 2;
		 */
		k4 = k2 - c1.getY();
		/*
		 * (X-Xa) ^ 2 + (K4 - K3X) ^ 2 = Ra^2; 化简得到:(K3^2+1)X ^ 2 - 2(Xa+K3*K4)*X +
		 * Xa^2 + K4^2 - Ra^2 = 0
		 */
		k5 = Math.pow(k3, 2) + 1;
		k6 = -2 * (c1.getX() + k3 * k4);
		k7 = Math.pow(c1.getX(), 2) + Math.pow(k4, 2) - Math.pow(r1, 2);
		// 等式化简得到 K5 * X^2 + 2*K6*X + (K7 - Ra^2)=0
		// 此时计算X的坐标
		k8 = Math.pow(k6, 2) - 4 * k5 * k7;
		p1 = Math.sqrt(k8);
		double x1, x2, y1, y2;
		if (k8 < 0) { // 没有任何交点
			x1 = x2 = y1 = y2 = -1;
			crossPoint.setSectionSituation(SectionSituation.NO);
		} else if (k8 == 0) {// 说明只有一个交点
			x1 = x2 = (double) (-2 * k6 + p1) / (double) (2 * k5);
			y1 = y2 = k2 - k3 * x1;
			Point[] points = { new Point(x1, y1) };
			crossPoint.setPoints(points);
			crossPoint.setSectionSituation(SectionSituation.TT1);
		} else { // 说明有两个交点
			x1 = (double) (-k6 + p1) / (double) (2 * k5);
			x2 = (double) (-k6 - p1) / (double) (2 * k5);
			y1 = k2 - k3 * x1;
			y2 = k2 - k3 * x2;
			Point[] points = { new Point(x1, y1),
			  new Point(x2, y2)
			};
			crossPoint.setPoints(points);
			crossPoint.setSectionSituation(SectionSituation.TT2);
		}
		return crossPoint;
	}
	/***
	 * 计算圆与直线的交点坐标
	 * @param cx
	 * @param cy
	 * @param r
	 * @param stx
	 * @param sty
	 * @param edx
	 * @param edy
	 * @return
	 */
	public static List<Point> circleIntersectLine(double cx, double cy, double r, 
			double stx, double sty, double edx, double edy) {
		List<Point> locals = new ArrayList<Point>();
        double x1, y1, x2, y2;
        if (edx - stx == 0) { //说明与x轴垂直
            double b0 = r * r - (stx - cx) * (stx - cx);
            if (b0 < 0) { // 无交点
                return locals;
            } else if (b0 == 0) { // 有一个交点
                locals.add(new Point(stx, cy));
            } else {  // 有两个交点
                x1 = stx;
                y1 = cy + Math.sqrt(b0);
                x2 = stx;
                y2 = cy - Math.sqrt(b0);
                locals.add(new Point(x1, y1));
                locals.add(new Point(x2, y2));
            }
            return locals;
        } else if (edy - sty == 0) { // 说明与y轴垂直 
            double b2 = r * r - (edy - cy) * (edy - cy);
            if (b2 < 0) { // 没有任何交点
                return locals;
            } else if (b2 == 0) { // 只有一个交点
                locals.add(new Point(cx, sty));
            } else { // 有两个交点
                x1 = cx + Math.sqrt(b2);
                y1 = sty;
                x2 = cx - Math.sqrt(b2);
                y2 = sty;
                locals.add(new Point(x1, y1));
                locals.add(new Point(x2, y2));
            }
            return locals;
        } else {
            double k = (edy - sty) / (edx - stx);
            double b = edy - k * edx;
            //列方程
            double c = cx * cx + (b - cy) * (b - cy) - r * r;
            double a = (1 + k * k);
            double b1 = (2 * cx - 2 * k * (b - cy));
            double jp = b1 * b1 - 4 * a * c;
            if (jp < 0) { // 无解
                return locals;
            }
            double tmp = Math.sqrt(jp);
            x1 = (b1 + tmp) / (2 * a);
            y1 = (k * x1 + b);
            x2 = (b1 - tmp) / (2 * a);
            y2 = (k * x2 + b);
            //判断求出的点是否在圆上
            if (jp == 0) {
                locals.add(new Point(x1, y1));
            } else {
                locals.add(new Point(x1, y1));
                locals.add(new Point(x2, y2));
            }
            return locals;
        }
	}
	/***
	 * 计算在房间的位置
	 * @param roomPath
	 * @param roomNode
	 * @param refPos
	 * @return
	 */
	public static Point calculateRoomPlace(BranchPath roomPath, BranchNode roomNode, ENodeBDistance refPos) {
        float pathLong = (float)pixelPitch(roomPath.getStart(), roomPath.getEnd());
        float scale = pathLong / roomNode.getMaxDistance();
        if (refPos.getDistance() > 0) {
        	List<Point> pts = circleIntersectLine(roomPath.getEnd().getX(), roomPath.getEnd().getY(), refPos.getDistance() * scale,
            roomPath.getStart().getX(), roomPath.getStart().getY(), roomPath.getEnd().getX(), roomPath.getEnd().getY());
            boolean mark1 = onSegment(pts.get(0), roomPath);
            boolean mark2 = onSegment(pts.get(1), roomPath);
            // 只有两种情况：没有交点和只有一个交点
            if (!mark1 && !mark2) {
                // 没有交点我们返回一个空坐标
                return new Point(-1, -1);
            } else {
                return mark1 ? pts.get(0) : pts.get(1);
            }
        } else {
            // 上报的距离基站的距离为 0 
            return roomPath.getEnd();
        }
    }
	/***
	 * 是否在路径外面
	 * @param place
	 * @param line
	 * @return
	 */
	public static boolean isOut(Point place, BasePath line)
    {
        Vector pV = new Vector(place.getX(), place.getY());
        Vector aV = new Vector(line.getStart().getX(), line.getStart().getY());
        Vector bV = new Vector(line.getEnd().getX(), line.getEnd().getY());
        Vector v1 = bV.sub(aV), v2 = pV.sub(aV), v3 = pV.sub(bV);
        if (Vector.dcmp(v1.dot(v2)) < 0) {    
        	return true;
        }
        else if (Vector.dcmp(v1.dot(v3)) > 0) {
            return true;
        }
        return false;
    }
	/***
	 * 计算X
	 * @param m
	 * @param n
	 * @param x1
	 * @param y1
	 * @param x2
	 * @param y2
	 * @return
	 */
	private static double calculateX(double m, double n, double x1, 
            double y1, double x2, double y2) {
        return ((m * (x2 - x1) * (x2 - x1) + n * (y2 - y1) * (x2 - x1) + (x1 * y2 - x2 * y1) * (y2 - y1)) / 
          ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)));
    }
	/***
	 * 计算Y
	 * @param m
	 * @param n
	 * @param x1
	 * @param y1
	 * @param x2
	 * @param y2
	 * @return
	 */
	private static double calculateY(double m, double n, double x1,
            double y1, double x2, double y2) {
        return ((m * (x2 - x1) * (y2 - y1) + n * (y2 - y1) * (y2 - y1) + (x2 * y1 - x1 * y2) * (x2 - x1)) / 
          ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)));
    }
	/***
	 * 计算点在直线上的投影
	 * @param line
	 * @param place
	 * @return
	 */
	public static Point UVProject(BasePath line, Point place) {
        if (isOut(place, line)) { // 点在线段外面
            double sd1 = pixelPitch(line.getStart(), place);
            double sd2 = pixelPitch(line.getEnd(), place);
            return sd1 <= sd2 ? line.getStart() : line.getEnd();
        } else {
            // 点在线段里面
            double px = calculateX(place.getX(), place.getY(), line.getStart().getX(), 
            				       line.getStart().getY(), line.getEnd().getX(), 
            				       line.getEnd().getY());
            double py = calculateY(place.getX(), place.getY(), 
            					   line.getStart().getX(), line.getStart().getY(), 
            					   line.getEnd().getX(), line.getEnd().getY());
            return new Point(px, py);
        }
    }
	/***
	 * 按照距离对基站上报的距离进行排序
	 * @param basictags
	 */
	public static void bubbleSort_ClosestDistance(List<ENodeBDistance> basictags)
    {
		ENodeBDistance tempbasic = new ENodeBDistance();
        for (int i = basictags.size() - 1; i >= 0; --i)
        {
            for (int j = 0; j < i; ++j)
            {
                if (basictags.get(j).getDistance() > basictags.get(j + 1).getDistance())
                {
                	copyENodeBDistance(basictags.get(j), tempbasic);
                	copyENodeBDistance(basictags.get(j + 1), basictags.get(j));
                	copyENodeBDistance(tempbasic, basictags.get(j + 1));
                }
            }
        }
    }
	/***
	 * 拷贝
	 * @param brtag
	 * @param copytag
	 */
	private static void copyENodeBDistance(ENodeBDistance brtag, ENodeBDistance copytag) {
        copytag.setId(brtag.getId());
        copytag.setPriority(brtag.getPriority());
        copytag.setDistance(brtag.getDistance());
    }
}
